#include "StdAfx.h"
#include "SweepCircleVoronoi.h"


vector<CSweepCircleVoronoi::Point> CSweepCircleVoronoi::sites;
vector<CSweepCircleVoronoi::Vertex> CSweepCircleVoronoi::vertexPool;
vector<CSweepCircleVoronoi::Edge> CSweepCircleVoronoi::edgePool;
CSweepCircleVoronoi::Point CSweepCircleVoronoi::center;
CSweepCircleVoronoi::Point CSweepCircleVoronoi::minPoint;
CSweepCircleVoronoi::Point CSweepCircleVoronoi::maxPoint;
const float CSweepCircleVoronoi::EllipseArc::tolerance(0.5 * FLT_EPSILON);

CSweepCircleVoronoi::CSweepCircleVoronoi(const vector<Point>& inputsites, Point center)
{
	sites = inputsites;
	CSweepCircleVoronoi::center = center;
	Initialize();
}

CSweepCircleVoronoi::~CSweepCircleVoronoi(void)
{
	Clean();
}

void CSweepCircleVoronoi::Initialize()
{
	Clean();

	for (int i = 0; i < (int)sites.size(); ++i)
	{
		Event evt;
		evt.type = NewEllipse;
		evt.site = sites[i];
		float keyDis = sqrt((center.x - sites[i].x) * (center.x - sites[i].x) + (center.y - sites[i].y) * (center.y - sites[i].y));
		eventQue.push(priority_structure<float, Event>(keyDis, evt));
	}

	distanceGap = 0.01;
	if (!eventQue.empty() && eventQue.top().Key() > distanceGap)
		distanceGap = eventQue.top().Key() ;
}

void CSweepCircleVoronoi::Clean()
{
	distance = 0;
	distanceGap = 0;

	vertexPool.clear();
	edgePool.clear();
	while (!eventQue.empty())
	{
		eventQue.pop();
	}
	headArc = NULL;
	distance = 0;

	TRACE("all arcs %d\n" , toBeDeletedArcs.size()  );
	for (vector<EllipseArc*>::iterator it = toBeDeletedArcs.begin(); it != toBeDeletedArcs.end(); ++it)
	{
		delete *it;
	}
	toBeDeletedArcs.clear();

	TRACE("all edge %d\n" , toBeDeletedEdges.size());
	for (vector<ActiveEdge*>::iterator it = toBeDeletedEdges.begin(); it != toBeDeletedEdges.end(); ++it)
	{
		delete *it;
	}
	toBeDeletedEdges.clear();

	headArc = NULL;
}

void CSweepCircleVoronoi::HandleOneLevel()
{


	cout << "The current distance: " << eventQue.top().Key() << endl;
	if (headArc == NULL)
	{
		float dOfFirstLevel = eventQue.top().Key();
		priority_queue<priority_structure<float, EllipseArc*> > arcsOfFirstLevel;
		priority_structure<float, Event> lastEvt = eventQue.top();
		while (!eventQue.empty() && eventQue.top().Data().type == NewEllipse
			&& eventQue.top().Key() - dOfFirstLevel < FLT_EPSILON)
		{
			EllipseArc* arc = new EllipseArc(center, eventQue.top().Data().site, NULL, NULL);
			if (headArc == NULL)
				headArc = arc;
			toBeDeletedArcs.push_back(arc);
			arcsOfFirstLevel.push(priority_structure<float, EllipseArc*>(arc->GetRotateAngle(), arc));
			lastEvt = eventQue.top();
			eventQue.pop();
		}
		eventQue.push(lastEvt);
	}
	else if (eventQue.top().Data().type == NewEllipse)
	{
		float angleSite = atan2(eventQue.top().Data().site.y - center.y, eventQue.top().Data().site.x - center.x);
		EllipseArc * arc = headArc;
		bool find = false;
		bool fEntireCircle = (headArc->leftActiveEdge == NULL);
		bool fCloseToLeft = false;
		bool fCloseToRight = false;
#ifdef DEBUG
		TRACE("-------------angleSite: %lf\n", angleSite);
		do
		{
			Point angles = arc->GetAngles(eventQue.top().Key());
			TRACE("anglei: (%lf, %lf)\n", angles.x, angles.y);			
		}
		while (arc->leftActiveEdge != NULL && (arc = arc->leftActiveEdge->leftArc) != headArc);
		TRACE("-----------------------------\n");
		arc = headArc;
#endif
		do
		{
			Point angles = arc->GetAngles(eventQue.top().Key());
			TRACE("anglei: (%lf, %lf)\n", angles.x, angles.y);
			float tmpAngle = angleSite;
			if (tmpAngle < angles.x)
				tmpAngle += 2 * M_PI;
			if (tmpAngle >= angles.x && tmpAngle <= angles.y)
			{
				find = true;
				if (tmpAngle - angles.x < FLT_EPSILON)
					fCloseToRight = true;
				if (tmpAngle - angles.y > -FLT_EPSILON)
					fCloseToLeft = true;
				break;
			}
			if (arc->leftActiveEdge == NULL)
				arc = NULL;
			else
				arc = arc->leftActiveEdge->leftArc;
		}
		while (arc != NULL && arc != headArc);
		if (!find)
		{
			assert(find);
		}

		EllipseArc* arcNew = new EllipseArc(center, eventQue.top().Data().site, NULL, NULL);
		toBeDeletedArcs.push_back(arcNew);
		Vertex intersection;
		if (arc->GetP(eventQue.top().Key()) < FLT_EPSILON)
		{
			intersection = center;
		}
		else
		{
			intersection = arc->GetPointAtAngleParam(eventQue.top().Key(), angleSite);
		}
		vertexPool.push_back(intersection);
		int nV = vertexPool.size() - 1;
		if (fEntireCircle)
		{
			ActiveEdge* activeEdge1 = new ActiveEdge(arc, arcNew, nV);
			toBeDeletedEdges.push_back(activeEdge1);
			ActiveEdge* activeEdge2 = new ActiveEdge(arcNew, arc, nV);
			toBeDeletedEdges.push_back(activeEdge2);
			arc->leftActiveEdge = activeEdge2;
			arc->rightActiveEdge = activeEdge1;
			arcNew->leftActiveEdge = activeEdge1;
			arcNew->rightActiveEdge = 	activeEdge2;				
		}
		else
		{
			if (fCloseToLeft)
			{
				EllipseArc* left = arc->leftActiveEdge->leftArc;
				Edge edge(left->rightActiveEdge->fixedVertex, nV);
				edgePool.push_back(edge);
				//delete left->rightActiveEdge;
				left->rightActiveEdge = new ActiveEdge(left, arcNew, nV);
				toBeDeletedEdges.push_back(left->rightActiveEdge);
				ActiveEdge* activeEdge = new ActiveEdge(arcNew, arc, nV);
				toBeDeletedEdges.push_back(activeEdge);
				arcNew->leftActiveEdge = left->rightActiveEdge;
				arcNew->rightActiveEdge = 	activeEdge;	
				arc->leftActiveEdge = activeEdge;
				pair<float, Event> result = left->GetVanishEvent();
				float keyDis = result.first;
				Event evt = result.second;
				if (keyDis < FLT_MAX)
				{
					eventQue.push(priority_structure<float, Event>(keyDis, evt));
				}
				result = arcNew->GetVanishEvent();
				keyDis = result.first;
				evt = result.second;
				if (keyDis < FLT_MAX)
				{
					eventQue.push(priority_structure<float, Event>(keyDis, evt));
				}
				result = arc->GetVanishEvent();
				keyDis = result.first;
				evt = result.second;
				if (keyDis < FLT_MAX)
				{
					eventQue.push(priority_structure<float, Event>(keyDis, evt));
				}
			}
			else if (fCloseToRight)
			{
				EllipseArc* right = arc->rightActiveEdge->rightArc;
				Edge edge(right->leftActiveEdge->fixedVertex, nV);
				edgePool.push_back(edge);
				//delete right->leftActiveEdge;
				right->leftActiveEdge = new ActiveEdge(arcNew, right, nV);
				toBeDeletedEdges.push_back(right->leftActiveEdge);
				ActiveEdge* activeEdge = new ActiveEdge(arc, arcNew, nV);
				toBeDeletedEdges.push_back(activeEdge);
				arcNew->rightActiveEdge = right->leftActiveEdge;
				arcNew->leftActiveEdge = activeEdge;	
				arc->rightActiveEdge = activeEdge;

				pair<float, Event> result = right->GetVanishEvent();
				float keyDis = result.first;
				Event evt = result.second;
				if (keyDis < FLT_MAX)
				{
					eventQue.push(priority_structure<float, Event>(keyDis, evt));
				}
				result = arcNew->GetVanishEvent();
				keyDis = result.first;
				evt = result.second;
				if (keyDis < FLT_MAX)
				{
					eventQue.push(priority_structure<float, Event>(keyDis, evt));
				}
				result = arc->GetVanishEvent();
				keyDis = result.first;
				evt = result.second;
				if (keyDis < FLT_MAX)
				{
					eventQue.push(priority_structure<float, Event>(keyDis, evt));
				}
			}
			else
			{
				EllipseArc* arc1 = arc;
				ActiveEdge* rightActiveEdge = arc->rightActiveEdge;
				EllipseArc* arc2 = new EllipseArc(*arc);
				toBeDeletedArcs.push_back(arc2);
				ActiveEdge* activeEdge1 = new ActiveEdge(arc1, arcNew, nV);
				toBeDeletedEdges.push_back(activeEdge1);
				ActiveEdge* activeEdge2 = new ActiveEdge(arcNew, arc2, nV);
				toBeDeletedEdges.push_back(activeEdge2);
				arc2->leftActiveEdge = activeEdge2;
				arc1->rightActiveEdge = activeEdge1;
				arcNew->leftActiveEdge = activeEdge1;
				arcNew->rightActiveEdge = 	activeEdge2;	
				arc2->rightActiveEdge = rightActiveEdge;
				if (rightActiveEdge != NULL)
				{
					rightActiveEdge->leftArc = arc2;
				}
				pair<float, Event> result = arc1->GetVanishEvent();
				float keyDis = result.first;
				Event evt = result.second;
				if (keyDis < FLT_MAX)
				{
					eventQue.push(priority_structure<float, Event>(keyDis, evt));
				}
				result = arc2->GetVanishEvent();
				keyDis = result.first;
				evt = result.second;
				if (keyDis < FLT_MAX)
				{
					eventQue.push(priority_structure<float, Event>(keyDis, evt));
				}
			}
		}		
	}
	else if (eventQue.top().Data().type == EllipseVanish)
	{
		float actualDis = eventQue.top().Key() + EllipseArc::tolerance;
		if (eventQue.top().Data().leftActiveEdge == eventQue.top().Data().arc->leftActiveEdge && eventQue.top().Data().rightActiveEdge == eventQue.top().Data().arc->rightActiveEdge)
		{
			Vertex v = eventQue.top().Data().site;
			vertexPool.push_back(v);
			int vIndex = vertexPool.size() - 1;
			headArc = eventQue.top().Data().arc->leftActiveEdge->leftArc;
			EllipseArc* leftArc = eventQue.top().Data().arc->leftActiveEdge->leftArc;
			//vector<ActiveEdge*> inactiveEdges;
			vector<EllipseArc*> inactiveArcs;
			bool flagHeadArcReplace = false;
			while (leftArc->Zero(actualDis))
			{
				inactiveArcs.push_back(leftArc);

				//inactiveEdges.push_back(leftArc->leftActiveEdge);				
				edgePool.push_back(Edge(leftArc->leftActiveEdge->fixedVertex,vIndex));
				leftArc = leftArc->leftActiveEdge->leftArc;
			}
			EllipseArc* rightArc = eventQue.top().Data().arc->rightActiveEdge->rightArc;
			while (rightArc->Zero(actualDis))
			{
				inactiveArcs.push_back(rightArc);
				//inactiveEdges.push_back(rightArc->rightActiveEdge);
				edgePool.push_back(Edge(rightArc->rightActiveEdge->fixedVertex,vIndex));
				rightArc = rightArc->rightActiveEdge->rightArc;
			}
			//inactiveEdges.push_back(eventQue.top().Data().arc->leftActiveEdge);
			//inactiveEdges.push_back(eventQue.top().Data().arc->rightActiveEdge);
			edgePool.push_back(Edge(eventQue.top().Data().arc->leftActiveEdge->fixedVertex,vIndex));
			edgePool.push_back(Edge(eventQue.top().Data().arc->rightActiveEdge->fixedVertex,vIndex));
			inactiveArcs.push_back(eventQue.top().Data().arc);

			ActiveEdge* newActiveEdge = new ActiveEdge(leftArc, rightArc, vIndex);
			toBeDeletedEdges.push_back(newActiveEdge);
			leftArc->rightActiveEdge = newActiveEdge;
			rightArc->leftActiveEdge = newActiveEdge;
/*			for (int i = 0; i < (int)inactiveEdges.size(); ++i)
			{
				Edge edge(inactiveEdges[i]->fixedVertex, vIndex);
				edgePool.push_back(edge);
			}*/


			for (int i = 0; i < (int)inactiveArcs.size(); ++i)
			{				
				inactiveArcs[i]->leftActiveEdge = inactiveArcs[i]->rightActiveEdge = NULL;
				if (inactiveArcs[i] == headArc){
					headArc = rightArc;
					
					TRACE("i %d\n");
					
				}
			}
			
			pair<float, Event> result = leftArc->GetVanishEvent();
			float keyDis = result.first;
			Event evt = result.second;
			if (keyDis < FLT_MAX)
			{
				eventQue.push(priority_structure<float, Event>(keyDis, evt));
			}
			result = rightArc->GetVanishEvent();
			keyDis = result.first;
			evt = result.second;
			if (keyDis < FLT_MAX)
			{
				eventQue.push(priority_structure<float, Event>(keyDis, evt));
			}
		}
	}
	float previousD = eventQue.top().Key();
	eventQue.pop();
	float nextD = GetNextEvent().first;

	if (nextD < FLT_MAX)
	{
		distanceGap = nextD - previousD > 0.01f ? nextD - previousD : 0.01f;
	}
}

bool CSweepCircleVoronoi::BuildVoronoiDiagram()
{
	RemoveOutofdateEvents();
	if (eventQue.empty())
		return false;	
	distance = eventQue.top().Key();
	HandleOneLevel();
	TRACE("the current distance: %lf\n", distance);
	return true;
}

bool CSweepCircleVoronoi::BuildVoronoiDiagram(float d)
{
	if (eventQue.empty() || eventQue.top().Key() > FLT_MAX)
	{
		return false;
	}
	while (!eventQue.empty() && d >= eventQue.top().Key())
	{
		HandleOneLevel();
	}
	distance = d;
	TRACE("the current distance: %lf\n", distance);
	return true;
}

void CSweepCircleVoronoi::RemoveOutofdateEvents()
{
	while (!eventQue.empty())
	{
		if (eventQue.top().Key() > FLT_MAX)
		{
			eventQue.pop();
		}
		else if (eventQue.top().Data().type  == NewEllipse)
		{
			break;
		}
		//EllipseVanish
		if (eventQue.top().Data().leftActiveEdge == eventQue.top().Data().arc->leftActiveEdge && eventQue.top().Data().rightActiveEdge == eventQue.top().Data().arc->rightActiveEdge)
		{
			break;
		}
		else
		{
			//delete eventQue.top().arc; // when delete????
			eventQue.pop();
		}
	}
}

pair<float, CSweepCircleVoronoi::Event> CSweepCircleVoronoi::GetNextEvent()
{
	CSweepCircleVoronoi::Event evt;
	float keyDis = FLT_MAX;
	evt.arc = NULL;
	evt.leftActiveEdge = evt.rightActiveEdge = NULL;
	evt.site;
	evt.type = -1;
	RemoveOutofdateEvents();
	if (eventQue.empty())
		return pair<float, Event>(keyDis, evt);

	keyDis = eventQue.top().Key();
	evt = eventQue.top().Data();
	return pair<float, Event>(keyDis, evt);
}
